#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>
#include <unistd.h>
#include <signal.h>
#include <semaphore.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include <sys/time.h>
#include <GL/glut.h>
#include <GL/freeglut.h>
#include <netpbm/pam.h>
#include "bzrc.h"

#define IP_ADDRESS "127.0.0.1"
#define FLAG_RADIUS 3.0f

#define SPEED_SCALAR 1.8f

#define FLAG_STEP 0.7f // this is alpha
#define OBSTACLE_STEP 0.65f // this is beta
#define TANGENT_STEP 0.01f // beta, but for tangent field

#define FLAG_SPREAD 40.0f
#define OBSTACLE_SPREAD 60.0f
#define TANGENT_SPREAD 0.1f

// Numerical Constants
#define PI 3.14159265358979323846
#define F_INFINITY 50

#define WORLD_UPDATE_RATE 200000
#define AGENT_UPDATE_RATE 200000

#define STUCK_THRESHOLD 20
#define ADJUSTMENT_MAGNITUDE 100.0f

// Globals
extern int debug;
int running;
sem_t mutex;

// Grid filter globals (to be packed in a struct later)
float* grid;
int width;
int height;
pair* destinations;
int destinationCount;
float pTruePositive;
float pTrueNegative;
int subdivSize = 80;

typedef struct {
	FILE* fSocket;
	flag* flags;
	int flagCount;
	tank* tanks;
	int tankCount;
	base* bases;
	int baseCount;
        obstacle* obstacles;
        int obstacleCount;
	constant* constants;
	int constantCount;
} world_info;

typedef struct {
	FILE* fSocket;
	int tankID;
	world_info* world;
        int flagGoalID;
} param;

// Thread functions
void *driver(void* args);
void *updater(void* args);
void *visualizer(void* args);
void *gunner(void* args);

// Potential field functions
pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed);
pair avoidObstacle(float x, float y, float xo, float yo, float radiuso);
pair circleObstacle(float x, float y, float xo, float yo, float radiusObs);
float move(FILE* fSocket, int tankID, float angle, float previousAngle, pair delta, float speed, float timeDiff);

// Helper functions
void intHandler();
float getDistanceBetween(float x, float y, float xg, float yg);
float getAngleBetween(float x, float y, float xg, float yg);
float normalizeAngle(float angle);
void drawGrid(void);
int timeval_subtract(struct timeval *result, struct timeval *t2, struct timeval *t1);
void setGrid(int x, int y, float intensity);
void showTank(pair location);
void updateGrid(occgrid sensorGrid);
void setConstants(world_info* world);
void makeDestinations(void);
int getStartDestinationIndex(int tankID, world_info* world);
int getEndDestinationIndex(int tankID, world_info* world, int startDestinationIndex);
int getNextDestinationIndex(int tankID, world_info* world, int currentDestinationIndex, int endDestinationIndex);
void simulatePath();
void createGridImage(char* filename, float* grid, int width, int height);

int main(int argc, char* argv[]) {
	int tcpSocket = 0;
	FILE* fSocket = 0;
	pthread_t* threads;
	pthread_t worldThread;
	pthread_t visualizerThread;
	param* threadArgs;
	world_info world;
	int threadCount;
	int i;
	running = 1;
	signal(SIGINT, intHandler);
	signal(SIGKILL, intHandler);
	sem_init(&mutex, 0, 0);
	if (argc < 2) {
		fprintf(stderr, "usage: gf_agent numagents port [debug 1:0]\n");
		exit(-1);
	}
	if (argc == 4 && atoi(argv[3]) == 1) {
		debug = 1;
	}
	threadCount = atoi(argv[1]);
	threads = (pthread_t*)malloc(sizeof(pthread_t) * threadCount);
	threadArgs = (param*)malloc(sizeof(param) * threadCount);
	BZRCConnect(IP_ADDRESS, ((argc == 2) ? 50000 : atoi(argv[2])), &tcpSocket, &fSocket);
	world.fSocket = fSocket;
	pthread_create(&worldThread, NULL, updater, (void*)&world);
	sem_wait(&mutex); // Wait for world data to be populated
	pthread_create(&visualizerThread, NULL, visualizer, (void*)&world);
	sem_wait(&mutex); // Wait for visualer data to be populated
	for (i = 0; i < threadCount; i++) {
		threadArgs[i].fSocket = fSocket;
		threadArgs[i].tankID = i;
		threadArgs[i].world = &world;
		threadArgs[i].flagGoalID = i % 3;
		pthread_create(&threads[i], NULL, driver, (void*)&threadArgs[i]);
	}

	for (i = 0; i < threadCount; i++) {
		pthread_join(threads[i], NULL);
	}
	pthread_join(worldThread, NULL);
	pthread_join(visualizerThread, NULL);
	BZRCDisconnect(tcpSocket, fSocket);
	sem_destroy(&mutex);
	free(threadArgs);
	free(threads);
	printf("Agents shut down\n");

	createGridImage("output.pgm", grid, width, height);
	free(grid);
	return 0;
}

void createGridImage(char* filename, float* grid, int width, int height) {
	FILE* outFile;
	gray** image;
	int i;
	int j;
	image = pgm_allocarray(width, height);
	for (i = 0; i < height; i++) {
		for (j = 0; j < width; j++) {
			image[height-i][j] = (gray)PGM_MAXMAXVAL * grid[i*width+j];
		}
	}
	outFile = fopen(filename, "w");
	pgm_writepgm(outFile, image, width, height, PGM_MAXMAXVAL, 1);
	fclose(outFile);
	pgm_freearray(image, height);
	return;
}

void drawGrid() {
	int draw = 0;
	glRasterPos2f(-1, -1);
	glDrawPixels(width, height, GL_LUMINANCE, GL_FLOAT, grid);
	glFlush();
	glutSwapBuffers();
	glutPostRedisplay();
	draw++;
	if (draw > 50) glutLeaveMainLoop();
	if (!running) glutLeaveMainLoop();
}

void showTank(pair location) {
	int x = (int)location.x + (height / 2);
	int y = (int)location.y + (width / 2);
	int i;
	int j;
	for (i = -1; i <= 1; i++) {
		for (j = -1; j <= 1; j++) {
			setGrid(x+i, y+j, 1.0f);
		}
	}
}

void setConstants(world_info* world) {
	int i;
	for (i = 0; i < world->constantCount; i++) {
		if (strcmp(world->constants[i].name, "worldsize") == 0) {
			height = atoi(world->constants[i].value);
			width = atoi(world->constants[i].value);
			printf("World size: %d,%d\n",height,width);
		}
		else if (strcmp(world->constants[i].name, "truepositive") == 0) {
			pTruePositive = atof(world->constants[i].value);
			printf("True Positive: %f\n",pTruePositive);
		}
		else if (strcmp(world->constants[i].name, "truenegative") == 0) {
			pTrueNegative = atof(world->constants[i].value);
			printf("True Negative: %f\n",pTrueNegative);
		}
	}
	return;
}

void makeDestinations() {
	int i;
	int j;
	int horizontalGoals = width/subdivSize;
	int verticalGoals = height/subdivSize;
	destinationCount = horizontalGoals * verticalGoals;
	destinations = (pair*)malloc(sizeof(pair) * destinationCount);
	for (i = 0; i < horizontalGoals; i++) {
		for (j = 0; j < verticalGoals; j++) {
			destinations[j*horizontalGoals+i].x = i*subdivSize+(subdivSize/2) - width/2;
			destinations[j*horizontalGoals+i].y = j*subdivSize+(subdivSize/2) - height/2;
			//setGrid(i*subdivSize+(subdivSize/2), j*subdivSize+(subdivSize/2), 1.0f);
		}
	}
}

void setGrid(int x, int y, float intensity) {
	if (x < 0 || x >= height) return;
	if (y < 0 || y >= width) return;
	grid[y*width+x] = intensity;
}

void updateGrid(occgrid sensorGrid) {
	int xstart = sensorGrid.topleft.x + (height / 2);
	int ystart = sensorGrid.topleft.y + (width / 2);
	int xend = xstart + sensorGrid.size.x;
	int yend = ystart + sensorGrid.size.y;
	int i;
	int j;
	float bel_occ;
	float bel_unocc;
	for (i = xstart; i < xend; i++) {
		for (j = ystart; j < yend; j++) {
			if (sensorGrid.grid[(i-xstart)*(int)sensorGrid.size.y+(j-ystart)] == '1') {
				bel_occ = pTruePositive * grid[j*width+i];
				bel_unocc = (1.0f - pTrueNegative) * (1.0f - grid[j*width+i]); 
			}
			else {
				bel_occ = (1.0f - pTruePositive) * grid[j*width+i];
				bel_unocc = pTrueNegative * (1.0f - grid[j*width+i]);
			}
			grid[j*width+i] = bel_occ / (bel_occ + bel_unocc);
			if (grid[j*width+i] > 1.0f) grid[j*width+i] = 1.0f;
			if (grid[j*width+i] < 0.0f) grid[j*width+i] = 0.0f;
		}
	}
	return;
}

int isLeftRightTank(int tankID, world_info* world) {
	return tankID < (world->tankCount / 2) ? 1 : 0;
}

int getStartDestinationIndex(int tankID, world_info* world) {
	int startDestinationIndex;
	int tankPartySize = world->tankCount / 2;
	int horizontalGoals = width/subdivSize;
	if (isLeftRightTank(tankID, world)) {
		startDestinationIndex = (destinationCount / tankPartySize) * tankID;
	}
	else {
		startDestinationIndex = ((horizontalGoals) / tankPartySize) * (tankID - tankPartySize);
	}
	return startDestinationIndex;
}

int getEndDestinationIndex(int tankID, world_info* world, int startDestinationIndex) {
	int endDestinationIndex;
	int horizontalGoals = width/subdivSize;
	int tankPartySize = world->tankCount / 2;
	if (isLeftRightTank(tankID, world)) {
		endDestinationIndex = startDestinationIndex + (destinationCount / tankPartySize) -1;
	}
	else {
		endDestinationIndex = startDestinationIndex + destinationCount - ((horizontalGoals) / tankPartySize) -1;
	}
	return endDestinationIndex;
}

int getNextDestinationIndex(int tankID, world_info* world, int currentDestinationIndex, int endDestinationIndex) {
	int nextDestinationIndex;
	int horizontalGoals = width/subdivSize;
	if (isLeftRightTank(tankID, world)) {
		nextDestinationIndex = currentDestinationIndex + 1;
		if (nextDestinationIndex == endDestinationIndex+1) {
			nextDestinationIndex = getStartDestinationIndex(tankID, world);
		}
	}
	else {
		nextDestinationIndex = currentDestinationIndex + horizontalGoals;
		if (nextDestinationIndex == endDestinationIndex+horizontalGoals) {
			nextDestinationIndex = getStartDestinationIndex(tankID, world);
		}
		else if (nextDestinationIndex > endDestinationIndex) {
			nextDestinationIndex = (currentDestinationIndex % horizontalGoals) + 1;
		}
	}
	return nextDestinationIndex;
}

void simulatePath() {
	int tankID = 0;
	int tankCount = 4;
	world_info virtualWorld;
	virtualWorld.tankCount = tankCount;
	int currentIndex;
	int startIndex = getStartDestinationIndex(tankID, &virtualWorld);
	currentIndex = startIndex;
	int endIndex = getEndDestinationIndex(tankID, &virtualWorld, currentIndex);
	double intensity = 0.05;
	int i = 0;
	setGrid(destinations[currentIndex].x + width/2, destinations[currentIndex].y + height/2, intensity);
	if (isLeftRightTank(tankID, &virtualWorld)) {
		printf("Tank %d goes left and right\n",tankID);
	}
	else {
		printf("Tank %d goes up and down\n",tankID);
	}
	while (1) {
		currentIndex = getNextDestinationIndex(tankID, &virtualWorld, currentIndex, endIndex);
		if (currentIndex == startIndex) break;
		intensity += 0.02;
		setGrid(destinations[currentIndex].x + width/2, destinations[currentIndex].y + height/2, intensity);
		printf("currentIndex: %d\n", currentIndex);
		if (destinations[currentIndex].x + width/2 == 400 && destinations[currentIndex].y + height/2 == 400) break;
		i++;
	}
	return;
}


void *driver(void* args) {
	param* params = (param*)args;
	FILE* fSocket = params->fSocket;
	world_info* world = params->world;
	int tankID = params->tankID;
	//pthread_t gunnerThread;
	//pthread_create(&gunnerThread, NULL, gunner, args);
	pair destination;
	pair totalDelta;
	pair currentDelta;
	float speed;
	float previousAngle = 0;
	struct timeval tvBegin, tvEnd, tvDiff;
	occgrid sensorGrid;
	int destinationIndex = getStartDestinationIndex(tankID, world);
	int destinationEndIndex = getEndDestinationIndex(tankID, world, destinationIndex);
	float distanceToDestination;

	// Stuck
	int timesStuckXPos = 0;
	int timesStuckXNeg = 0;
	int timesStuckYPos = 0;
	int timesStuckYNeg = 0;
	pair curLoc;
	pair prevLoc = world->tanks[tankID].location;
	pair prevDelta;
	prevDelta.x = 0.0f;
	prevDelta.y = 0.0f;

	while (running) {
		// Sleep
		gettimeofday(&tvBegin, NULL);
		usleep(AGENT_UPDATE_RATE);
		gettimeofday(&tvEnd, NULL);
		timeval_subtract(&tvDiff, &tvEnd, &tvBegin);

		// Update world view
		if (getOccGrid(fSocket, &sensorGrid, tankID)) {
			printf("Failed to get occgrid for tank: %d\n", tankID);
			return 0;
		}
		updateGrid(sensorGrid);
		free(sensorGrid.grid);

		//showTank(world->tanks[tankID].location);
		curLoc = world->tanks[tankID].location;
		destination.x = destinations[destinationIndex].x;
		destination.y = destinations[destinationIndex].y;
		distanceToDestination = getDistanceBetween(curLoc.x, curLoc.y, destination.x, destination.y);
		if (distanceToDestination < 43.0f) {
			destinationIndex = getNextDestinationIndex(tankID, world, destinationIndex, destinationEndIndex);
			destination.x = destinations[destinationIndex].x;
			destination.y = destinations[destinationIndex].y;
		}

		// Plot path to destination
		currentDelta = seekGoal(
			world->tanks[tankID].location.x,
			world->tanks[tankID].location.y,
			destination.x,
			destination.y,
			FLAG_RADIUS,
			&speed
		);
		totalDelta.x = currentDelta.x;
		totalDelta.y = currentDelta.y;

		if (fabs(prevLoc.x - curLoc.x) < 0.05 && prevDelta.x > +0.0f) {
			timesStuckXPos++;
			timesStuckXNeg = 0;
			//printf("timesStuckXPos: %d\n",timesStuckXPos);
		}
		else {
			timesStuckXPos = 0;
		}
		if (fabs(prevLoc.x - curLoc.x) < 0.05 && prevDelta.x < -0.0f) {
			timesStuckXNeg++;
			timesStuckXPos = 0;
			//printf("timesStuckXNeg: %d\n",timesStuckXNeg);
		}
		else {
			timesStuckXNeg = 0;
		}

		if (fabs(prevLoc.y - curLoc.y) < 0.05 && prevDelta.y > +0.0f) {
			timesStuckYPos++;
			timesStuckYNeg = 0;
			//printf("timesStuckYPos: %d\n",timesStuckYPos);
		}
		else {
			timesStuckYPos = 0;
		}
		if (fabs(prevLoc.y - curLoc.y) < 0.05 && prevDelta.y < -0.0f) {
			timesStuckYNeg++;
			timesStuckYPos = 0;
			//printf("timesStuckYNeg: %d\n",timesStuckYNeg);
		}
		else {
			timesStuckYNeg = 0;
		}

		if (timesStuckXPos > STUCK_THRESHOLD && timesStuckYPos > STUCK_THRESHOLD) {
			//printf("stuck going top right\n");
			totalDelta.y = -ADJUSTMENT_MAGNITUDE;
			totalDelta.x = ADJUSTMENT_MAGNITUDE;
		}
		else if (timesStuckXPos > STUCK_THRESHOLD && timesStuckYNeg > STUCK_THRESHOLD) {
			//printf("stuck going bottom right\n");
			totalDelta.x = -ADJUSTMENT_MAGNITUDE;
			totalDelta.y = -ADJUSTMENT_MAGNITUDE;
		}
		else if (timesStuckXNeg > STUCK_THRESHOLD && timesStuckYPos > STUCK_THRESHOLD) {
			//printf("stuck going top left\n");
			totalDelta.x = ADJUSTMENT_MAGNITUDE;
			totalDelta.y = ADJUSTMENT_MAGNITUDE;
		}
		else if (timesStuckXNeg > STUCK_THRESHOLD && timesStuckYNeg > STUCK_THRESHOLD) {
			//printf("stuck going bottom left\n");
			totalDelta.y = ADJUSTMENT_MAGNITUDE;
			totalDelta.x = -ADJUSTMENT_MAGNITUDE;
		}
		else if (timesStuckXPos > STUCK_THRESHOLD) {
			//printf("stuck going right\n");
			totalDelta.y = -ADJUSTMENT_MAGNITUDE;
			totalDelta.x = 0.1f;
		}
		else if (timesStuckXNeg > STUCK_THRESHOLD) {
			//printf("stuck going left\n");
			totalDelta.y = ADJUSTMENT_MAGNITUDE;
			totalDelta.x = -0.1f;
		}
		else if (timesStuckYPos > STUCK_THRESHOLD) {
			//printf("stuck going up\n");
			totalDelta.x = ADJUSTMENT_MAGNITUDE;
			totalDelta.y = 0.1f;
		}
		else if (timesStuckYNeg > STUCK_THRESHOLD) {
			//printf("stuck going down\n");
			totalDelta.x = -ADJUSTMENT_MAGNITUDE;
			totalDelta.y = -0.1f;
		}

		speed = sqrt(pow(totalDelta.x,2) + pow(totalDelta.y,2)) / 20;
		previousAngle = move(
			fSocket,
			tankID,
			world->tanks[tankID].angle,
			previousAngle,
			totalDelta,
			speed,
			(float)tvDiff.tv_usec
		);
		prevLoc = curLoc;
		prevDelta = totalDelta;
	}
	setSpeed(fSocket, tankID, 0);
	setAngVel(fSocket, tankID, 0);
	//pthread_join(gunnerThread, NULL);
	return 0;
}

void *gunner(void* args) {
	param* params = (param*)args;
	FILE* fSocket = params->fSocket;
	int tankID = params->tankID;
	int sleeptime;
	while (running) {
		shoot(fSocket, tankID);
		sleeptime = (rand() % 1000000) + 1500000;
		usleep(sleeptime);
	}
	return 0;
}


// Some shared memory problems and memory leak problems exist here
// They'd be easy to remove if they became a problem.  I'll do it later.
// world structures need to be freed when not in use by any tank, and
// world needs to be locked by updater thread when it performs updates
void *updater(void* args) {
	FILE* fSocket;
	world_info* world = (world_info*)args;
	fSocket = world->fSocket;
	//getFlags(fSocket, &world->flags, &world->flagCount);
	getMyTanks(fSocket, &world->tanks, &world->tankCount);
	//getBases(fSocket, &world->bases, &world->baseCount); // bases are static, don't update
	//getObstacles(fSocket, &world->obstacles, &world->obstacleCount);
	getConstants(fSocket, &world->constants, &world->constantCount);
	sem_post(&mutex);
	while (running) {
		usleep(WORLD_UPDATE_RATE);
		//free(world->flags);
		//getFlags(fSocket, &world->flags, &world->flagCount);
		free(world->tanks);
		getMyTanks(fSocket, &world->tanks, &world->tankCount);
	}
	return 0;
}

void *visualizer(void* args) {
	FILE* fSocket;
	world_info* world = (world_info*)args;
	fSocket = world->fSocket;
	int argc = 0;
	char** argv = 0;
	int i;
	int j;
	setConstants(world);
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA | GLUT_DEPTH);
	glutInitWindowSize(width, height);
	glutInitWindowPosition(0, 0);
	glutCreateWindow("Grid Filter");
	grid = (float*)malloc(sizeof(float) * width * height);
	for (i = 0; i < height; i++) {
		for (j = 0; j < width; j++) {
			grid[i*width+j] = 0.75f;
			//grid[i*width+j] = 0.0f;
		}
	}
	makeDestinations();
	//simulatePath();
	sem_post(&mutex);
	glutSetOption(GLUT_ACTION_ON_WINDOW_CLOSE, GLUT_ACTION_CONTINUE_EXECUTION);
	glutDisplayFunc(drawGrid);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	signal(SIGINT, intHandler); // have to re-register these handlers since glut overwrote them
	signal(SIGKILL, intHandler);
	glutMainLoop();
	return 0;
}

pair seekGoal(float x, float y, float xg, float yg, float radiusg, float* retSpeed) {
	float distance = getDistanceBetween(x, y, xg, yg);
	float angle = getAngleBetween(x, y, xg, yg);
	float speed = 0;
	pair delta;
	delta.x = 0;
	delta.y = 0;
	if (distance < radiusg) {
		delta.x = 0;
		delta.y = 0;
		speed = 0;
	}
	else if (radiusg <= distance && distance <= (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * (distance - radiusg) * cos(angle);
		delta.y = FLAG_STEP * (distance - radiusg) * sin(angle);
		speed = sqrt(pow(delta.x,2) + pow(delta.y,2));
	}
	else if (distance > (FLAG_SPREAD + radiusg)) {
		delta.x = FLAG_STEP * FLAG_SPREAD * cos(angle);
		delta.y = FLAG_STEP * FLAG_SPREAD * sin(angle);
		speed = 1.0;
	}
	*retSpeed = speed;
	return delta;
}

pair avoidObstacle(float x, float y, float xo, float yo, float radiusObs) {
	float distance = getDistanceBetween(x, y, xo, yo);
	float angle = getAngleBetween(x, y, xo, yo);
	pair delta;
	delta.x = 0;
	delta.y = 0;

	if (distance < radiusObs) {
		delta.x = -cos(angle) * F_INFINITY;
		delta.y = -sin(angle) * F_INFINITY;
	}
	else if (radiusObs <= distance && distance <= (OBSTACLE_SPREAD + radiusObs)) {
		delta.x = -OBSTACLE_STEP * (OBSTACLE_SPREAD + radiusObs - distance) * cos(angle);
		delta.y = -OBSTACLE_STEP * (OBSTACLE_SPREAD + radiusObs - distance) * sin(angle);
	}
	else if (distance > (OBSTACLE_SPREAD + radiusObs)) {
		delta.x = 0;
		delta.y = 0;
	}

	return delta;
}

pair circleObstacle(float x, float y, float xo, float yo, float radiusObs) {
	float distance = getDistanceBetween(x, y, xo, yo);
	float angle = getAngleBetween(x, y, xo, yo) + 90;
	pair delta;
	delta.x = 0;
	delta.y = 0;

	if (distance < radiusObs) {
		delta.x = -cos(angle) * F_INFINITY;
		delta.y = -sin(angle) * F_INFINITY;
	}
	else if (radiusObs <= distance && distance <= (TANGENT_SPREAD + radiusObs)) {
		delta.x = -TANGENT_STEP * (TANGENT_SPREAD + radiusObs - distance) * cos(angle);
		delta.y = -TANGENT_STEP * (TANGENT_SPREAD + radiusObs - distance) * sin(angle);
	}
	else if (distance > (TANGENT_SPREAD + radiusObs)) {
		delta.x = 0;
		delta.y = 0;
	}

	return delta;
}

float move(FILE* fSocket, int tankID, float currentAngle, float previousAngle, pair delta, float speed, float timeDiff) {
	float goalAngle = atan2(delta.y, delta.x);
	currentAngle = normalizeAngle(currentAngle);
	float errorAngle = goalAngle - currentAngle;
	float newAngle = errorAngle + ((errorAngle - previousAngle) / timeDiff);
	if (newAngle > PI) {
		newAngle = newAngle - 2 * PI;
	}
	else if (newAngle < -PI) {
		newAngle = 2 * PI + newAngle;
	}

	speed = SPEED_SCALAR * (-pow((fabs(newAngle / PI)), (float)1.0f/4.0f) + 1);
	//printf("speed: %02.1f\n",speed);
	setSpeed(fSocket, tankID, speed);
	setAngVel(fSocket, tankID, newAngle / PI);
	return newAngle;
}

float normalizeAngle(float angle) {
	float normalizedAngle;
	normalizedAngle = fmod(angle,(2 * PI));
	if (normalizedAngle >= PI) {
		normalizedAngle -= (2 * PI);
	}
	else if (normalizedAngle <= -PI) {
		normalizedAngle += (2 * PI);
	}
	return normalizedAngle;
}

float getDistanceBetween(float x, float y, float xg, float yg) {
	float distance;
	distance = sqrt(pow((x - xg),2) + pow((y - yg),2));
	return distance;
}

float getAngleBetween(float x, float y, float xg, float yg) {
	float angle;
	angle = atan2((yg - y),(xg - x));
	return angle;
}

void intHandler() {
	printf("Shutting down agents...\n");
	running = 0;
	return;
}

int timeval_subtract(struct timeval *result, struct timeval *t2, struct timeval *t1) {
    long int diff = (t2->tv_usec + 1000000 * t2->tv_sec) - (t1->tv_usec + 1000000 * t1->tv_sec);
    result->tv_sec = diff / 1000000;
    result->tv_usec = diff % 1000000;

    return (diff<0);
}


